﻿using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Data.SqlTypes;
using System.IO;
using System.Linq;
using System.Threading;
using static MAVLink;

namespace DroneLib.Communication
{
    public abstract class MavStream : Stream, IDisposable
    {
        internal string _Name;
        public string Name => _Name;

        public override bool CanRead => true;

        public override bool CanSeek => false;

        public override bool CanWrite => true;

        public override long Position { get => throw new NotSupportedException(); set => throw new NotSupportedException(); }

        protected bool _Connected = false;
        public virtual bool Connected => _Connected;

        public virtual bool IsOpen { get; protected set; } = false;

        public bool ConnectionFailed { get; protected set; } = false;
        public string Error { get; protected set; } = string.Empty;

        public int MavlinkVersion { get; set; } = 2;

        public override long Seek(long offset, SeekOrigin origin)
        {
            throw new NotSupportedException();
        }

        public override void SetLength(long value)
        {
            throw new NotSupportedException();
        }

        public abstract void Open();
        public abstract override void Close();

        public abstract void Reconnect();

        public delegate void LinkBrokenEventHandler(MavStream mavStream);
        public event LinkBrokenEventHandler LinkBroken;

        public delegate void LinkConnectedEventHandler(MavStream mavStream, bool connected);
        public event LinkConnectedEventHandler LinkConnected;

        public byte GcsID { get; set; } = 255;

        Dictionary<byte, IMessageReciver> _MessageConsumer = new Dictionary<byte, IMessageReciver>();


        protected bool _TerminateLoop = true;
        protected void StartMessageLoop()
        {
            if (_TerminateLoop == false)
                return;

            _TerminateLoop = false;

            new Thread(ThdRxLoop) { IsBackground = true }.Start();
            new Thread(ThdRxMsgProcessLoop) { IsBackground = true }.Start();
            new Thread(ThdSendHeartbeat) { IsBackground = true }.Start();
        }

        protected void StopMessageLoop()
        {
            _TerminateLoop = true;
        }


        ConcurrentQueue<MAVLink.MAVLinkMessage> _FreeMessage = new ConcurrentQueue<MAVLink.MAVLinkMessage>();
        public bool FreeMessageAvailable => _FreeMessage.Count > 0;

        ConcurrentQueue<MAVLink.MAVLinkMessage> _RxMessage = new ConcurrentQueue<MAVLink.MAVLinkMessage>();

        MAVLink.MavlinkParse _Parse = new MAVLink.MavlinkParse();

        protected void ThdRxLoop()
        {
            while (_TerminateLoop==false)
            {
                MAVLink.MAVLinkMessage msg = null;
                try
                {
                    msg = _Parse.ReadPacket(this);
                    //Console.WriteLine($"{this.Length} bytes left in stream");
                }
                catch(Exception ex)
                {
                    if (this is UdpStream==false && this.Connected == false)
                    {
                        RaiseLinkBrokenEvent();
                        StopMessageLoop();
                        break;
                    }
                }
                if (msg == null)
                {
                    continue;
                }
                else
                {
                    //Console.WriteLine($">>>>>>>>>>>>>>>> {(MAVLink.MAVLINK_MSG_ID)msg.msgid}");
                    _RxMessage.Enqueue(msg);
                }
            }
        }

        protected void ThdRxMsgProcessLoop()
        {
            MAVLink.MAVLinkMessage msg;
            while (_TerminateLoop == false)
            {
                if(_RxMessage.Count==0)
                {
                    Thread.Sleep(10);
                    continue;
                }


                while(_RxMessage.Count>0)
                {
                    if (_RxMessage.TryDequeue(out msg) == false)
                        break;

                    //Console.WriteLine($"**************** {(MAVLink.MAVLINK_MSG_ID)msg.msgid}");

                    if (_MessageConsumer.ContainsKey(msg.sysid))
                    {
                        if (_WaitingMessage && _WaitingForMsgIdList.Contains(msg.msgid))
                        {
                            lock (_ResponseMessageLock)
                                _ResponseMessage.Add(msg);
                        }
                        else if (_MessageConsumer[msg.sysid].ProcessMessage(msg))
                        {
                            continue;
                        }
                    }

                    try
                    {
                        lock (Subscriptions)
                        {
                            foreach (var vt in Subscriptions)
                            {
                                if ((UInt32)vt.msgId == msg.msgid)
                                {
                                    vt.function(msg);
                                }
                            }
                        }
                    }
                    catch(Exception ex)
                    {

                    }

                    _FreeMessage.Enqueue(msg);
                    if (_FreeMessage.Count > 2000)
                    {
                        MAVLink.MAVLinkMessage m = null;
                        for (int i = 0; i < 100; i++)
                            _FreeMessage.TryDequeue(out m);
                    }
                }
            }
        }

        bool _WaitingMessage = false;
        public bool WaitingMessage => _WaitingMessage;
        object _WaitMessageLock = new object();
        //UInt16 _WaitingForMsgId;
        List<UInt32> _WaitingForMsgIdList = new List<UInt32>();
        public bool Responsed => _ResponseMessage.Count > 0;
        //MAVLink.MAVLinkMessage _ResponseMessage = null;
        List<MAVLink.MAVLinkMessage> _ResponseMessage = new List<MAVLink.MAVLinkMessage>();
        object _ResponseMessageLock = new object();
        public bool StartWaitMessage(params MAVLink.MAVLINK_MSG_ID[] msgId)
        {
            if (_WaitingMessage)
            {
                foreach (var id in msgId)
                {
                    if (_WaitingForMsgIdList.Contains((UInt32)id) == false)
                        return false;
                }
            }
            else
            {
                lock (_WaitMessageLock)
                {
                    if (_WaitingMessage)
                        return false;

                    _WaitingMessage = true;
                    lock (_ResponseMessageLock)
                        _ResponseMessage.Clear();
                    _WaitingForMsgIdList.Clear();
                    _WaitingForMsgIdList.AddRange(msgId.Select(v => (UInt32)v));
                }
            }            
            return true;
        }

        public void StopWaitMessage()
        {
            lock (_WaitMessageLock)
            {
                _WaitingMessage = false;
                _WaitingForMsgIdList.Clear();
            }
        }

        public MAVLink.MAVLinkMessage ResponseMessage
        {
            get
            {
                if (Responsed)
                {
                    MAVLink.MAVLinkMessage msg = null;
                    if (_ResponseMessage.Count > 0)
                    {
                        lock (_ResponseMessageLock)
                        {
                            if (_ResponseMessage.Count > 0)
                            {
                                msg = _ResponseMessage[0];
                                _ResponseMessage.RemoveAt(0);
                            }
                        }
                    }
                    return msg;
                }
                else
                {
                    return null;
                }  
            }
        }

        public MAVLink.MAVLinkMessage ReadFreeMessage(MAVLink.MAVLINK_MSG_ID msgId)
        {
            MAVLink.MAVLinkMessage m = null;
            while (_FreeMessage.Count > 0)
            {
                if (_FreeMessage.TryDequeue(out m))
                {
                    if (m.msgid == (UInt32)msgId)
                        return m;
                }
            }
            return null;
        }

        public void ClearFreeMessage()
        {
            MAVLink.MAVLinkMessage m = null;
            while (_FreeMessage.Count > 0)
            {
                _FreeMessage.TryDequeue(out m);
            }
        }

        int _SendFailure = 0;
        public void SendMessage(MAVLink.MAVLINK_MSG_ID msgType,object data)
        {
            byte[] bytes;
            if (MavlinkVersion == 2)
                bytes = _Parse.GenerateMAVLinkPacket20(msgType, data, false, GcsID);
            else
                bytes= _Parse.GenerateMAVLinkPacket10(msgType, data, GcsID);
            
            //MAVLink.MAVLinkMessage msg = new MAVLink.MAVLinkMessage(bytes);
            try
            {
                this.Write(bytes, 0, bytes.Length);
                _SendFailure = 0;
            }
            catch
            {
                _SendFailure++;
                if (this is UdpStream == false && _SendFailure > 5)
                {
                    RaiseLinkBrokenEvent();
                }
            }
            //Console.WriteLine($"Send msg:[{(MAVLink.MAVLINK_MSG_ID)msg.msgid}]: SEQ = {msg.seq}");
        }

        protected void RaiseLinkConnectedEvent(bool connected)
        {
            LinkConnected?.Invoke(this, connected);
        }

        protected void RaiseLinkBrokenEvent()
        {
            _Connected = false;
            IsOpen = false;
            try
            {
                this.Close();
            }
            catch { }

            LinkBroken?.Invoke(this);
        }

        public void MessageConsumerRegister(byte sysId,IMessageReciver receiver)
        {
            _MessageConsumer[sysId] = receiver;
        }




        readonly private List<(MAVLINK_MSG_ID msgId, Func<MAVLinkMessage, bool> function, bool exclusive, byte sysid, byte compid)> Subscriptions =
       new List<(MAVLINK_MSG_ID, Func<MAVLinkMessage, bool>, bool, byte, byte)>();

        /// <summary>
        /// Subscribe to a packet on the current target MAV. use OnPacketReceived to get all MAVs
        /// </summary>
        /// <param name="type"></param>
        /// <param name="function"></param>
        /// <param name="exclusive"></param>
        /// <returns></returns>
        public int SubscribeToPacketType(MAVLINK_MSG_ID msgid,
            Func<MAVLinkMessage, bool> function, byte sysid, byte compid, bool exclusive = false)
        {
            var item = (msgid, function, exclusive, sysid, compid);

            lock (Subscriptions)
            {
                if (exclusive)
                {
                    foreach (var subitem in Subscriptions)
                    {
                        if (subitem.msgId == item.msgid)
                        {
                            Subscriptions.Remove(subitem);
                            break;
                        }
                    }
                }

                Subscriptions.Add(item);
            }

            return item.GetHashCode();
        }

        public void UnSubscribeToPacketType(int id)
        {
            lock (Subscriptions)
            {
                var sub = Subscriptions.ToArray();

                foreach (var valueTuple in sub)
                {
                    if (id == valueTuple.GetHashCode())
                    {
                        Subscriptions.Remove(valueTuple);
                        break;
                    }
                }
            }
        }







        private void ThdSendHeartbeat()
        {
            MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t(0, (byte)MAVLink.MAV_TYPE.GCS, 0, 0, 0, 2);
            while (_TerminateLoop == false)
            {
                if (_Connected && IsOpen)
                    SendMessage(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
                Thread.Sleep(1000);
            }
        }
    }
}
